#include "soft_i2c.h"

#define I2C_SDA_OUT()	ctrl->gpio_set_direction(ctrl->sda, SOFT_I2C_GPIO_OUT)
#define I2C_SDA_IN()	ctrl->gpio_set_direction(ctrl->sda, SOFT_I2C_GPIO_IN)
#define I2C_SCL_OUT()	ctrl->gpio_set_direction(ctrl->scl, SOFT_I2C_GPIO_OUT)
#define I2C_SCL_IN()	ctrl->gpio_set_direction(ctrl->scl, SOFT_I2C_GPIO_IN)
#define I2C_SDA_HIGH()	ctrl->gpio_set_level(ctrl->sda, SOFT_I2C_GPIO_HIGH)
#define I2C_SDA_LOW()	ctrl->gpio_set_level(ctrl->sda, SOFT_I2C_GPIO_LOW)
#define I2C_SCL_HIGH()	ctrl->gpio_set_level(ctrl->scl, SOFT_I2C_GPIO_HIGH)
#define I2C_SCL_LOW()	ctrl->gpio_set_level(ctrl->scl, SOFT_I2C_GPIO_LOW)
#define I2C_SDA_READ()	ctrl->gpio_get_level(ctrl->sda)
#define I2C_DELAY(us)	ctrl->delay_us(us)

static void soft_i2c_gpio_init(soft_i2c_ctrl *ctrl)
{
	I2C_SDA_OUT();
	I2C_SCL_OUT();
	I2C_SDA_HIGH();
	I2C_SCL_HIGH();
	I2C_DELAY(20000);
}

static void soft_i2c_start(soft_i2c_ctrl *ctrl)
{
	I2C_SDA_OUT();
	
	I2C_SDA_HIGH();
	I2C_SCL_HIGH();
	I2C_DELAY(4);
	
	I2C_SDA_LOW();
	I2C_DELAY(4);
	I2C_SCL_LOW();
	I2C_DELAY(4);
}

static void soft_i2c_stop(soft_i2c_ctrl *ctrl)
{
	I2C_SDA_OUT();
	
	I2C_SDA_LOW();
	I2C_SCL_LOW();
	I2C_DELAY(4);
	
	I2C_SCL_HIGH();
	I2C_DELAY(4);
	I2C_SDA_HIGH();
	I2C_DELAY(4);
}

static void soft_i2c_ack(soft_i2c_ctrl *ctrl)
{
	I2C_SDA_OUT();
	
	I2C_SCL_LOW();
	I2C_DELAY(4);
	I2C_SDA_HIGH();
	I2C_DELAY(4);
	
	I2C_SDA_LOW();
	I2C_DELAY(4);
	I2C_SCL_HIGH();
	I2C_DELAY(4);
	I2C_SCL_LOW();
	I2C_DELAY(4);
	I2C_SDA_HIGH();
}

static void soft_i2c_nack(soft_i2c_ctrl *ctrl)
{
	I2C_SDA_OUT();
	
	I2C_SCL_LOW();
	I2C_DELAY(4);
	I2C_SDA_HIGH();
	I2C_DELAY(4);
	
	I2C_SCL_HIGH();
	I2C_DELAY(4);
	I2C_SCL_LOW();
	I2C_DELAY(4);
}

static bool soft_i2c_wait_ack(soft_i2c_ctrl *ctrl)
{
	I2C_SCL_LOW();
	//I2C_SDA_HIGH();
	//I2C_DELAY(4);
	I2C_SDA_IN();
	I2C_SCL_HIGH();
	I2C_DELAY(4);
	
	int timeout = 0;
	while (I2C_SDA_READ()) {
		timeout++;
		if (timeout >= 250) {
			ctrl->i2c_stop(ctrl);
			return false;
		}
	}
	
	I2C_SCL_LOW();
	I2C_DELAY(4);
	I2C_SDA_OUT();
	I2C_SDA_HIGH();
	
	return true;
}

static void soft_i2c_write_8bit(soft_i2c_ctrl *ctrl, uint8_t byte)
{
	I2C_SDA_OUT();
	
	for (int i = 7; i >= 0; i--) {
		if ((byte >> i) & 0x01) {
			I2C_SDA_HIGH();
		} else {
			I2C_SDA_LOW();
		}
		
		I2C_DELAY(4);
		I2C_SCL_HIGH();
		I2C_DELAY(4);
		I2C_SCL_LOW();
		I2C_DELAY(4);
	}
}

static uint8_t soft_i2c_read_8bit(soft_i2c_ctrl *ctrl)
{
	I2C_SCL_LOW();
	I2C_DELAY(4);
	I2C_SDA_HIGH();
	I2C_SDA_IN();
	
	int i = 8, b = 0;
	for (; i > 0; i--) {
		b = b << 1;
		I2C_SCL_HIGH();
		I2C_DELAY(4);
		if (I2C_SDA_READ()) {
			b |= 0x01;
		}
		I2C_SCL_LOW();
		I2C_DELAY(4);
	}
	
	return b;
}

static int __soft_i2c_transfer(soft_i2c_ctrl *ctrl, uint8_t chip, soft_i2c_xfer *xfer)
{
	if (!(xfer->flag & SOFT_I2C_NOSTART)) {
		ctrl->i2c_start(ctrl);
	}
	
	ctrl->write_8bit(ctrl, (xfer->flag & SOFT_I2C_RD) ? ((chip << 1) | 0x01) : (chip << 1));
	
	if (!ctrl->i2c_wait_ack(ctrl)) return -1;
	
	if (xfer->flag & SOFT_I2C_RD) {
		for (int i = 0; i < xfer->len; i++) {
			xfer->buf[i] = ctrl->read_8bit(ctrl);
			
			if (i == xfer->len - 1) {
				ctrl->i2c_nack(ctrl);
			} else {
				ctrl->i2c_ack(ctrl);
			}
		}
	} else {
		for (int i = 0; i < xfer->len; i++) {
			ctrl->write_8bit(ctrl, xfer->buf[i]);
			
			if (!ctrl->i2c_wait_ack(ctrl)) return -1;
		}
	}
	
	if (!(xfer->flag & SOFT_I2C_NOSTOP)) {
		ctrl->i2c_stop(ctrl);
	}
	
	return 0;
}

static int __soft_i2c_transfers(soft_i2c_ctrl *ctrl, uint8_t chip, soft_i2c_xfer *xfer, uint32_t len)
{
	for (int i = 0; i < len; i++) {
		if (__soft_i2c_transfer(ctrl, chip, xfer + i)) {
			return -1;
		}
	}
	
	return 0;
}

static int __soft_i2c_read_reg(soft_i2c_ctrl *ctrl, uint8_t chip, uint8_t reg, uint8_t *byte)
{
	ctrl->i2c_start(ctrl);
	
	ctrl->write_8bit(ctrl, (chip << 1));
	if (!ctrl->i2c_wait_ack(ctrl)) return -1;
	
	ctrl->write_8bit(ctrl, reg);
	if (!ctrl->i2c_wait_ack(ctrl)) return -1;
	
	ctrl->i2c_start(ctrl);
	
	ctrl->write_8bit(ctrl, (chip << 1) | 0x01);
	if (!ctrl->i2c_wait_ack(ctrl)) return -1;
	
	*byte = ctrl->read_8bit(ctrl);
	ctrl->i2c_nack(ctrl);
	
	ctrl->i2c_stop(ctrl);
	
	return 0;
}

static int __soft_i2c_write_reg(soft_i2c_ctrl *ctrl, uint8_t chip, uint8_t reg, uint8_t byte)
{
	ctrl->i2c_start(ctrl);
	
	ctrl->write_8bit(ctrl, (chip << 1));
	if (!ctrl->i2c_wait_ack(ctrl)) return -1;
	
	ctrl->write_8bit(ctrl, reg);
	if (!ctrl->i2c_wait_ack(ctrl)) return -1;
	
	ctrl->write_8bit(ctrl, byte);
	if (!ctrl->i2c_wait_ack(ctrl)) return -1;
	
	ctrl->i2c_stop(ctrl);
	
	return 0;
}

void soft_i2c_init(soft_i2c_ctrl *ctrl)
{
	ctrl->gpio_init = soft_i2c_gpio_init;
	ctrl->i2c_start = soft_i2c_start;
	ctrl->i2c_stop = soft_i2c_stop;
	ctrl->i2c_ack = soft_i2c_ack;
	ctrl->i2c_nack = soft_i2c_nack;
	ctrl->i2c_wait_ack = soft_i2c_wait_ack;
	ctrl->write_8bit = soft_i2c_write_8bit;
	ctrl->read_8bit = soft_i2c_read_8bit;
	ctrl->transfer = __soft_i2c_transfers;
	ctrl->read_reg = __soft_i2c_read_reg;
	ctrl->write_reg = __soft_i2c_write_reg;
	
	ctrl->gpio_init(ctrl);
}